import math
import numpy as np

target_speed = 0.1

class Trajectory:
    def __init__(self, x, y, yaw, direction, kappa, v):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.direction = direction
        self.kappa = kappa
        self.v = v

def kappaCal(x, y):
    # 计算每个点处的导数 
    dx = np.gradient(x) 
    dy = np.gradient(y)
    # 计算每个点处的二阶导数
    ddx = np.gradient(dx)
    ddy = np.gradient(dy)
    # 计算每个点处的曲率半径
    k = np.abs(dx * ddy - dy * ddx) / (dx ** 2 + dy ** 2) ** 1.5
    k = k.tolist()

    return k

def pathPreProcess(path):
    l = 0.05 #
    len_n = 4
    interval = 0.025 # 每隔4厘米取一个点
    add_d = len_n * interval # 参数
    reverse_ind = []
    direct = path.direction[0]
    for i in range(len(path.x)):
        if path.direction[i] is not direct:
            direct = path.direction[i]
            reverse_ind.append(i - 1) # 尖点处索引值

    traj_list = []
    Point_O = []
    if len(reverse_ind):
        path_x = []
        path_y = []
        for i in range(len(reverse_ind) + 1):
            if i == 0:
                path_x1 = path.x[ : (reverse_ind[i] + 1)]
                path_y1 = path.y[ : (reverse_ind[i] + 1)]
            elif i == len(reverse_ind):
                path_x1 = path.x[(reverse_ind[i - 1] + 1) : ]
                path_y1 = path.y[(reverse_ind[i - 1] + 1) : ]
            else:
                path_x1 = path.x[(reverse_ind[i - 1] + 1) : (reverse_ind[i] + 1)]
                path_y1 = path.y[(reverse_ind[i - 1] + 1) : (reverse_ind[i] + 1)] 
            path_x.append(path_x1)
            path_y.append(path_y1) # 储存了n段路径

        for i in range(len(reverse_ind)):
            O = [path.x[reverse_ind[i]], path.y[reverse_ind[i]]] # 尖点坐标（圆心坐标）
            Point_O.append(O) # 将多个尖点储存到数组
            r_ind = reverse_ind[i] - 1
            f_ind = reverse_ind[i] + 1
            LA = math.hypot(O[0] - path.x[r_ind], O[1] - path.y[r_ind])
            LB = math.hypot(O[0] - path.x[f_ind], O[1] - path.y[f_ind])
            while LA < l:
                r_ind -= 1
                LA = math.hypot(O[0] - path.x[r_ind], O[1] - path.y[r_ind])
            while LB < l:
                f_ind += 1
                LB = math.hypot(O[0] - path.x[f_ind], O[1] - path.y[f_ind])
            A = [path.x[r_ind], path.y[r_ind]]
            B = [path.x[f_ind], path.y[f_ind]]
            D = [(A[0] + B[0]) / 2, (A[1] + B[1]) / 2] # 点A和点B的中点

            theta = math.atan2(O[1] - D[1], O[0] - D[0]) # 延长线偏航角
            P = [O[0] + add_d * math.cos(theta), O[1] + add_d * math.sin(theta)] # 求出延长线端点P坐标

            f_x_ = np.linspace(O[0], P[0], len_n, endpoint=True)
            # f_x_ = np.round(f_x_, 2)
            f_x_ = f_x_.tolist()
            f_x = f_x_[1 : ] # 延长线第一段  front
            f_y_ = np.linspace(O[1], P[1], len_n, endpoint=True)
            # f_y_ = np.round(f_y_, 2)
            f_y_ = f_y_.tolist()
            f_y = f_y_[1 : ] # 延长线第一段

            r_x = f_x_[::-1][1:] # 延长线第二段  rear
            r_y = f_y_[::-1][1:] # 反转数组并剔除端点

            if path.direction[reverse_ind[i]] == 1:
                factor = 1
            else:
                factor = -1  # 航向角是否需要偏转180°判据 1：否（先前进再倒车）， -1：是（先倒车再前进）
            yaw = math.atan2((O[1] - D[1]) * factor, (O[0] - D[0]) * factor)
            f_yaw = [yaw] * len(f_x)
            r_yaw = [yaw] * len(r_x)
            
            if i == 0:
                f_direction = [path.direction[int((0 + reverse_ind[0]) / 2)]] * len(f_x) # 确定direction的值
            else:
                f_direction = [path.direction[int((reverse_ind[i - 1] + 1 + reverse_ind[i]) / 2)]] * len(f_x)
            r_direction = [num * -1 for num in f_direction]

            path_x1 = path.x[ : (reverse_ind[i] + 1)]
            path_x2 = path.x[(reverse_ind[i] + 1): ]
            path_y1 = path.y[ : (reverse_ind[i] + 1)]
            path_y2 = path.y[(reverse_ind[i] + 1) : ] # 打断原始路径
            path_yaw1 = path.yaw[ : (reverse_ind[i] + 1)]
            path_yaw2 = path.yaw[(reverse_ind[i] + 1) : ]
            path_direction1 = path.direction[ : (reverse_ind[i] + 1)]
            path_direction2 = path.direction[(reverse_ind[i] + 1) : ]

            # 拼接路径
            path.x = path_x1 + f_x + r_x + path_x2
            path.y = path_y1 + f_y + r_y + path_y2
            path.yaw = path_yaw1 + f_yaw + r_yaw + path_yaw2
            path.direction = path_direction1 + f_direction + r_direction + path_direction2

            if i < len(reverse_ind) - 1:
                reverse_ind[i + 1] = reverse_ind[i + 1] + 2 * len(f_x)
            print("*********************")
        
        kappa = []
        f_kappa = [0] * len(f_x)
        r_kappa = [0] * len(r_x)
        for i in range(len(reverse_ind)):
            kappa1 = kappaCal(path_x[i], path_y[i])
            kappa += kappa1 + f_kappa + r_kappa
        kappa_last = kappaCal(path_x[len(reverse_ind)], path_y[len(reverse_ind)])
        kappa += kappa_last

        traj = Trajectory(path.x, path.y, path.yaw, path.direction, kappa, [target_speed] * len(path.x))
        # 分段轨迹
        index = []
        direct = traj.direction[0]
        for i in range(len(traj.x)):
            if traj.direction[i] is not direct:
                direct = traj.direction[i]
                index.append(i - 1) # 尖点处索引值
        for i in range(len(index) + 1):
            if i == 0:
                traj_x = traj.x[ : (index[i] + 1)]
                traj_y = traj.y[ : (index[i] + 1)]
                traj_yaw = traj.yaw[ : (index[i] + 1)]
                traj_direction = traj.direction[ : (index[i] + 1)]
                traj_kappa = traj.kappa[ : (index[i] + 1)]
                traj_v = traj.v[ : (index[i] + 1)]
            elif i == len(index):
                traj_x = traj.x[(index[i - 1] + 1) : ]
                traj_y = traj.y[(index[i - 1] + 1) : ]
                traj_yaw = traj.yaw[(index[i - 1] + 1) : ]
                traj_direction = traj.direction[(index[i - 1] + 1) : ]
                traj_kappa = traj.kappa[(index[i - 1] + 1) : ]
                traj_v = traj.v[(index[i - 1] + 1) : ]
            else:
                traj_x = traj.x[(index[i - 1] + 1) : (index[i] + 1)]
                traj_y = traj.y[(index[i - 1] + 1) : (index[i] + 1)]
                traj_yaw = traj.yaw[(index[i - 1] + 1) : (index[i] + 1)]
                traj_direction = traj.direction[(index[i - 1] + 1) : (index[i] + 1)]
                traj_kappa = traj.kappa[(index[i - 1] + 1) : (index[i] + 1)]
                traj_v = traj.v[(index[i - 1] + 1) : (index[i] + 1)]
            traj1 = Trajectory(traj_x, traj_y, traj_yaw, traj_direction, traj_kappa, traj_v)
            print("第", i +1, "段轨迹长度: ", len(traj1.x))
            traj_list.append(traj1)
    else:
        kappa = kappaCal(path.x, path.y)
        traj = Trajectory(path.x, path.y, path.yaw, path.direction, kappa, [target_speed] * len(path.x))
        traj_list.append(traj)
        print("轨迹只有1段")

    print("处理后的轨迹有", len(traj_list), "段", "总轨迹长度: ", len(traj.x))
        
    return traj, traj_list, Point_O

